package com.example.auv_660.autocontrolalgorithm;

import com.example.auv_660.MainActivity;
import com.example.auv_660.autocontrolalgorithm.assistantcalculator.AutoControl;
import com.example.auv_660.autocontrolalgorithm.assistantsensor.BatterySensor;
import com.example.auv_660.autocontrolalgorithm.assistantsensor.LocationSensor;
import com.example.auv_660.autocontrolalgorithm.assistantsensor.OrientationSensor;
import com.example.auv_660.autocontrolalgorithm.assistantsensor.SpeedSensor;
import com.example.auv_660.command.AUVBatteryCommand;
import com.example.auv_660.command.AUVDepthSensorCommand;
import com.example.auv_660.command.AUVPropeller;
import com.example.auv_660.network.udp.HFUDPBuild;
import com.example.auv_660.util.Constants;
import com.example.auv_660.util.FileUtils;
import com.example.auv_660.util.MsgType;
import com.example.auv_660.util.TaskUtils;
import com.example.auv_660.util.ThreadPool;

import java.io.BufferedReader;
import java.io.IOException;
import java.net.InetAddress;
import java.util.ArrayList;

public class AutoControlGPS extends Strategy {
    private MainActivity mainActivity = MainActivity.getInstance();
    private LocationSensor locationSensor;
    private OrientationSensor orientationSensor;
    private SpeedSensor speedSensor;
    private AUVPropeller.AUVPropellerCommand auvPropellerCommand;
    private AUVDepthSensorCommand auvDepthSensorCommand;
    private AUVBatteryCommand auvBatteryCommand;
    private BatterySensor batterySensor;
    private BufferedReader bufferedReader;
    private ArrayList<double[]> missionData;

    private double distance = 100;
    private double updateGPSLong = 0.0;
    private double updateGPSLat = 0.0;

    private double curLongitude;
    private double curLatitude;
    private double missionLongitude;
    private double missionLatitude;
    private double missionSpeed;
    private double missionDepth;
    private float[] orientation;
    private double[] speed;
    private int count = 0;
    private double refYaw = 0.0;
    private int warnOri = 0;

    @Override
    public void doOperation(String order) {
        mainActivity.sendMsgToMainThread(MsgType.CONTROLLER_MSG, "自主航行");
        openRelatedSensor();
        missionData = readMissionFile();
        Constants.ORDER = false;
        System.out.println("gps parameter: " + order);

        String[] parameters = order.split(",");

        if (parameters.length == 3)
        {
            Constants.SPEED = Double.parseDouble(parameters[1]);
            Constants.OPENGPSHELP = Integer.parseInt(parameters[2]);
        }

        try
        {
            Thread.sleep(100);
        }
        catch (InterruptedException e)
        {
            e.printStackTrace();
        }

        ThreadPool.INSTANCE.getThreadPoolProxy().joinTask(new Runnable() {
            @Override
            public void run() {
                Constants.RUN = true;

                while (locationSensor.getValue("GPS")[2] > Constants.GPS_ACCURACY
                        || locationSensor.getValue("GPS")[2] == 0.0)
                {
                    // TODO 超时处理
                    try
                    {
                        Thread.sleep(2000);
                    }
                    catch (InterruptedException e)
                    {
                        e.printStackTrace();
                    }

                    if (!Constants.RUN)
                    {
                        break;
                    }
                }

                curLongitude = locationSensor.getValue("GPS")[0];
                curLatitude = locationSensor.getValue("GPS")[1];

                System.out.println("GET GPS curlongitude: " + curLongitude);
                System.out.println("GET GPS curlatitude: " + curLatitude);
                System.out.println("GET GPS accuracy: " + locationSensor.getValue("GPS")[2]);

                if (curLatitude == 0.0 || curLongitude == 0.0)
                {
                    FileUtils.INSTANCE.getFileWrite().realseWriteDHData();
                    closeRelatedSensor();
                    Constants.ORDER = true;
                    Constants.OPENGPSHELP = 0;
                    return;
                }

                FileUtils.INSTANCE.getFileWrite().writeDHDATA("" + "Yaw" + " " + "Pitch" + " " + "Roll" +  " "
                        + "Depth" + " " + "TopRudder" + " "  + "BottomRudder" + " " + "LeftRudder" + " "
                        + "RightRudder" + " " + "Rotation" + " " + "CurLongtitude" + " " + "CurLatitude"
                        +  " " + "MissionLongtitude" + " " + "MissionLatitude" + " "  + "RealLongtitude"
                        + " " + "RealLatitude" + " "+  "Speed" +"\n");

                for (int i = 0; i < 100; i++)
                {
                    auvPropellerCommand.writeCommand(Constants.AUV_PROPELLER_ORDER_PREFIX + Constants.AUV_DOWN_WATER + Constants.AUV_ORDER_SUFFIX);
                    try
                    {
                        System.out.println("i: " + i);
                        Thread.sleep(100);
                        double curDepthAC = auvDepthSensorCommand.getDepthAC();
                        float[] orientation = orientationSensor.getValue();
                        curLongitude = locationSensor.getValue("GPS")[0];
                        curLatitude = locationSensor.getValue("GPS")[1];
                        FileUtils.INSTANCE.getFileWrite().writeDHDATA("" + orientation[0] + " " + orientation[1]
                                + " " + orientation[2] +  " " + curDepthAC + " " + "0" + " "  + "0" + " " + "15"
                                + " " + "-20" + " " + "3000" + " " + curLongitude + " " + curLatitude + " "
                                + missionLongitude + " " + missionLatitude + " " + curLongitude + " " + curLatitude + " "+ "0" + "\n");
                    }
                    catch (InterruptedException e)
                    {
                        e.printStackTrace();
                    }
                }

                System.out.println("missionData size: " + missionData.size());
                for (int i = 0; i < missionData.size(); i++)
                {
                    /**
                     此处整理认为源码存在错误 源码是：
                     missionLatitude = missionData.get(i)[0];
                     missionLongitude = missionData.get(i)[1];
                     */
                    missionLongitude = missionData.get(i)[0];
                    missionLatitude = missionData.get(i)[1];
                    missionSpeed = missionData.get(i)[2];
                    missionDepth = missionData.get(i)[3];

                    System.out.println("missionLatitude: " + missionLatitude);
                    System.out.println("missionLongtitude: " + missionLongitude);
                    System.out.println("missionSpeed: " + missionSpeed);
                    System.out.println("missionDepth: " + missionDepth);

                    long startMissionTime = System.currentTimeMillis();

                    while (distance > Constants.REACH_RADIUS && Constants.RUN)
                    {
                        // TODO 超时，超深处理，返航点出水
                        orientation = orientationSensor.getValue();

                        double[] values = locationSensor.getValue("GPS");

                        if (values[2] < Constants.GPS_ACCURACY)
                        {
                            if (updateGPSLong != values[0] || updateGPSLat != values[1])
                            {
                                updateGPSLong = values[0];
                                updateGPSLat = values[1];
                                String result = "C" + "Dot-" + values[0] + "," + values[1] + "-" + Constants.PHONEIMEI;
                                HFUDPBuild.INSTANCE.getHFUdpClient().sendMessage(result);

                                if (Constants.OPENGPSHELP == 1)
                                {
                                    curLongitude = values[0];
                                    curLatitude = values[1];
                                }
                            }
                        }

                        double[] position = AutoControl.calPosition(Constants.SPEED, 0, (360 - (double) orientation[0]), Constants.DT, curLongitude, curLatitude);
                        curLongitude = position[0];
                        curLatitude = position[1];

                        double[] gResult = AutoControl.guidance(curLongitude, curLatitude, missionLongitude, missionLatitude);
                        distance = gResult[0];

                        System.out.println("curYaw: " + orientation[0]);
                        System.out.println("gResult[1] R: " + (360 - gResult[1] * 180.0 / Math.PI));

                        String tailOrder = "";
                        double curDepthAC = auvDepthSensorCommand.getDepthAC();

                        double[] tailCtrl = AutoControl.tailSet(orientation[0], orientation[1], orientation[2], (360 - gResult[1] * 180.0 / Math.PI), curDepthAC, missionDepth, missionSpeed);

                        final double calYaw = 360 - gResult[1] * 180.0 / Math.PI;

                        mainActivity.runOnUiThread(new Runnable() {
                            @Override
                            public void run() {
                                mainActivity.updateGPScalData(distance, calYaw);    // TODO did not set the measured dimension by calling setMeasuredDimension()
                            }
                        });

                        if (tailCtrl == null)
                        {
                            return;
                        }

                        int length = tailCtrl.length;
                        for (int j = 0; j < length - 2; j++)
                        {
                            tailOrder += (int) tailCtrl[j] + ",";
                        }
                        tailOrder += (int) tailCtrl[length - 2];

                        String orderControl = Constants.AUV_PROPELLER_ORDER_PREFIX + tailOrder + Constants.AUV_ORDER_SUFFIX;

                        FileUtils.INSTANCE.getFileWrite().writeDHDATA("" + orientation[0] + " " + orientation[1] + " "
                                + orientation[2] +  " " + curDepthAC + " " + tailCtrl[0] + " "  + tailCtrl[1] + " "
                                + tailCtrl[2] + " " + tailCtrl[3] + " " + tailCtrl[4] + " " + curLongitude + " " + curLatitude
                                + " " + missionLongitude + " " + missionLatitude + " " + updateGPSLong + " " + updateGPSLat + " "+ "0" + "\n");

                        auvPropellerCommand.writeCommand(orderControl);

                        try
                        {
                            Thread.sleep(100);
                        }
                        catch (InterruptedException e)
                        {
                            e.printStackTrace();
                        }

                        System.out.println("distance: " + distance);

                        long curMissionTime = System.currentTimeMillis();

                        long duration;

                        if (i == 0)
                        {
                            duration = curMissionTime - startMissionTime + 10000;
                        }
                        else
                        {
                            duration = curMissionTime - startMissionTime;
                        }

                        if (duration > 30 * 60 * 1000)
                        {
                            if (mainActivity != null)
                            {
                                mainActivity.sendMsgToMainThread(MsgType.RED_CHAO_SHI, "");
                            }
                            Constants.DHWARNNING = 2;
                            Constants.RUN = false;
                            break;
                        }
                    }

                    if (!Constants.RUN)
                    {
                        break;
                    }

                    System.out.println("next mission");
                    auvPropellerCommand.writeCommand(Constants.AUV_PROPELLER_ORDER_PREFIX
                            + Constants.AUV_SELF_TEST_PROPELLER_ORDER_II + Constants.AUV_ORDER_SUFFIX);

                    distance = 100;

                    long begin = System.currentTimeMillis();
                    long cur = System.currentTimeMillis();

                    while ((cur - begin < 2 * 60 * 1000) && Constants.RUN)
                    {
                        try
                        {
                            Thread.sleep(1000);
                        }
                        catch (InterruptedException e)
                        {
                            e.printStackTrace();
                        }

                        cur = System.currentTimeMillis();

                        double[] values = locationSensor.getValue("GPS");

                        if (values[2] < Constants.GPS_ACCURACY)
                        {
                            System.out.println("GPS精度达标");
                            if (updateGPSLong != values[0] || updateGPSLat != values[1])
                            {
                                HFUDPBuild.INSTANCE.getHFUdpClient().sendMessage("CSPEAK:到达指定任务点");
                                System.out.println("CSPEAK:到达指定任务点");
                                updateGPSLong = values[0];
                                updateGPSLat = values[1];
                                curLongitude = values[0];
                                curLatitude = values[1];
                                break;
                            }
                        }
                    }
                }

                FileUtils.INSTANCE.getFileWrite().realseWriteDHData();
                closeRelatedSensor();

                boolean linkCheck = false;
                while (!linkCheck)
                {
                    try
                    {
                        linkCheck = InetAddress.getByName(Constants.SERVER_IP).isReachable(10000);
                    }
                    catch (IOException e)
                    {
                        e.printStackTrace();
                    }
                }

                HFUDPBuild.INSTANCE.getHFUdpClient().sendMessage("CENDTIME," + Constants.PHONEIMEI);
                Constants.ORDER = true;
                Constants.OPENGPSHELP = 0;
            }
        });
    }

    private void openRelatedSensor()
    {
        locationSensor = new LocationSensor();
        orientationSensor = new OrientationSensor();
        auvPropellerCommand = AUVPropeller.INSTANCE.getAuvPropellerCommand();
        auvDepthSensorCommand = new AUVDepthSensorCommand();
        auvBatteryCommand = new AUVBatteryCommand();
        batterySensor = new BatterySensor();
    }

    private void closeRelatedSensor()
    {
        locationSensor.stopLocation();
        orientationSensor.unregister();
        batterySensor.releaseSource();
    }

    private Boolean checkWarning()
    {
        float[] orientation = orientationSensor.getValue();

        if (orientation[0] == 0.0 && orientation[1] == 0.0 && orientation[2] == 0.0)
        {
            warnOri ++;
        }
        else
            {
            warnOri = 0;
        }

        if (warnOri == 50)
        {
            System.out.println("WARN: 1");
            Constants.RUN = false;
            return true;
        }

        double curDepthAC = auvDepthSensorCommand.getDepthAC();
        if (curDepthAC > Constants.CANNOTDEPTH)
        {
            System.out.println("WARN: 2");
            if (mainActivity != null && curDepthAC != 1000)
            {
                mainActivity.sendMsgToMainThread(MsgType.RED_CHAO_SHEN, "");
            }
            Constants.DHWARNNING = 1;
            Constants.RUN = false;
            return true;
        }

        String tmp = auvBatteryCommand.getBatteryValue();
        int auvBattery;
        if (tmp.equals("暂无电量显示"))
        {
            auvBattery = 0;
        }
        else
        {
            auvBattery = Integer.parseInt(tmp.split("%")[0]);
        }

        int phoneBattery = batterySensor.getCurrentBattery();

        if (auvBattery <= 20 || phoneBattery <= 20)
        {
            System.out.println("WARN: 3");
            Constants.RUN = false;
            Constants.DHWARNNING = 3;
            return true;
        }
        return false;
    }

    private ArrayList<double[]> readMissionFile()
    {
        TaskUtils taskUtils = new TaskUtils();
        bufferedReader = taskUtils.readTask();

        ArrayList<double[]> mission = new ArrayList<>();

        if (bufferedReader == null)
        {
            mainActivity.sendMsgToMainThread(MsgType.TOAST_MSG, "没有找到TASKINFORMATION文件！！！");
            System.out.println("没有找到TASKINFORMATION文件");
        }
        else
        {
            String line;

            try
            {
                line = bufferedReader.readLine();

                while (null != line)
                {
                    String[] information = line.split(":")[1].split(",");
                    double[] content = new double[4];
                    for (int i = 0; i < information.length; i++)
                    {
                        content[i] = Double.valueOf(information[i]);
                    }
                    mission.add(content);
                    line = bufferedReader.readLine();
                }
            }
            catch (IOException e)
            {
                taskUtils.realseReadTask();
                e.printStackTrace();
            }
        }
        taskUtils.realseReadTask();
        return mission;
    }
}
